
/*
 * main.cpp
 *
 *      Author: Christian
 *       Letztes Update: 11.01.14 Christian | fähler rausmachen
 *
 */




#ifndef F_CPU
#define F_CPU 16000000UL
#endif



#include <avr/io.h>
#include <util/delay.h>
#include <avr/interrupt.h>
#include <inttypes.h>
#include <avr/eeprom.h>


#include "MPU_6050.h"
#include "Feedback.h"
#include "UART.h"
#include "Empf.h"
#include "Scheduler.h"
#include "ppmGen.h"
#include "Kalman.h"
#include "regler.h"


	/* EEPROM Adressen
	 *
	 *	mode	| 0x00
	 *
	 * anteil_p | 0x01
	 * anteil_i | 0x02
	 * anteil_d | 0x03
	 *
	 * q_angle 	| 0x05
	 * q_gyro  	| 0x06
	 * r_angle 	| 0x07
	 * lowPass	| 0x08
	 *
	 * p_z 		| 0x09
	 * i_z 		| 0x0A
	 * d_z 		| 0x0B
	 *
	 */

// Empfangener Wert um in CONFIG_MODE zu springen
#define CONFIG_MODE 	5

void config_mode();
void normal_mode();

int main() {

 	if ( eeprom_read_byte( (uint8_t *) 0x00)  == 0x01) {
		config_mode();
	} else {
		normal_mode();
	}

	return 0;
}

void normal_mode() {
	// Initialisierung

	//uint8_t bla = 0;

	feedback_init();
	regler_init();
	mpu_init();
	UARTInit();
	empfaenger_init();
	ppmGenInit();
	kalman_init();

	scheduler_start();

	sei();

	// Programmschleife
		while ( 1 ) {

			if ( status & ( 1 << NEWSENS ) ) {
				kalman_update();

				calcMotorValues( empf_wert[1], 5 * ( empf_wert[2] - 50 ), 5 * ( empf_wert[0] - 50 ), 2 * (empf_wert[3] - 50 ));
				ppmSendSignal( motor_werte[0], motor_werte[1] , motor_werte[2], motor_werte[3] );


				status &= ~( 1 << NEWSENS );

			}

			// Debugging aufgaben

			if ( receive_buffer[0] == 1 ) {
				reset_receive_buffer();
				sendDebugMessage();


			} else if (  receive_buffer[0]  == 2 ) {
				reset_receive_buffer();
				reglerwerte_update();


			} else if ( receive_buffer[0]  == 3 ) {
				reset_receive_buffer();
				kalman_setup_update();

			} else if ( receive_buffer[0] == 4 ) {
				reset_receive_buffer();
				system_pause();
				system_reset();
			} else if (  (uint8_t) receive_buffer[0]  == CONFIG_MODE ) {
				reset_receive_buffer();
				system_pause();
				eeprom_update_byte( ( uint8_t *) 0x00, 0x01 );
				system_reset();
			}


		}
}


void config_mode() {

	// UART INIT
	UARTInit();

	//I2C INIT




	eeprom_update_byte( ( uint8_t *) 0x00, 0x00 );

	system_reset();
}
